
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       ahrs_sensor.c
  * @author     baiyang
  * @date       2021-11-21
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "ahrs_sensor.h"

#include <common/grapilot.h>
#include <common/time/gp_time.h>
#include <sensor_imu/sensor_imu.h>
#include <sensor_baro/sensor_baro.h>
#include <sensor_compass/sensor_compass.h>
#include <sensor_gps/sensor_gps.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void ahrs_sensor_init(uint16_t loop_rate)
{
    sensor_imu_ctor();
    sensor_imu_init(loop_rate);

    sensor_baro_ctor();
    sensor_baro_init();
    sensor_baro_calibrate(true);

    sensor_compass_ctor();
    sensor_compass_init();

    sensor_gps_ctor();
    sensor_gps_init();
}

void ahrs_sensor_update(uint32_t now_ms)
{
    TIMETAG_CHECK_EXECUTE2(imu, PERIOD_MS_100HZ, now_ms, sensor_imu_update();)
    TIMETAG_CHECK_EXECUTE2(baro, PERIOD_MS_10HZ, now_ms, sensor_baro_update();)
    TIMETAG_CHECK_EXECUTE2(compass, PERIOD_MS_10HZ, now_ms, sensor_compass_read();)
    TIMETAG_CHECK_EXECUTE2(compass_cal, PERIOD_MS_10HZ, now_ms, sensor_compass_cal_update();)
    TIMETAG_CHECK_EXECUTE2(gps, PERIOD_MS_50HZ, now_ms, sensor_gps_update();)
}

/*------------------------------------test------------------------------------*/


